#include <iostream>
#include <random>
#include <dds/dds.h>
#include "StartYsdd.h" // IDL生成的C语言头文件
#include "../../../include/log_client/LogClient.h"

// DDS实体句柄
static dds_entity_t participant;
static dds_entity_t topic;
static dds_entity_t publisher;
static dds_entity_t writer;

// 清理DDS资源
static void cleanup()
{
    if (writer != DDS_HANDLE_NIL)
        dds_delete(writer);
    if (publisher != DDS_HANDLE_NIL)
        dds_delete(publisher);
    if (topic != DDS_HANDLE_NIL)
        dds_delete(topic);
    if (participant != DDS_HANDLE_NIL)
        dds_delete(participant);
}

// 生成随机预设轨迹数据
StartYsdd_PresetTrajectoryInput generate_random_trajectory()
{
    static std::mt19937 rng(std::random_device{}());

    // 定义各参数的合理范围
    std::uniform_real_distribution<double> lat_dist(-90.0, 90.0);       // 纬度
    std::uniform_real_distribution<double> lon_dist(-180.0, 180.0);     // 经度
    std::uniform_real_distribution<double> alt_dist(0.0, 30000.0);      // 高度 (m)
    std::uniform_real_distribution<double> vel_dist(0.0, 1000.0);       // 速度 (m/s)
    std::uniform_real_distribution<double> heading_dist(0.0, 360.0);    // 航向 (度)
    std::uniform_real_distribution<double> max_vel_dist(500.0, 2000.0); // 最大速度 (m/s)
    std::uniform_real_distribution<double> max_accel_dist(5.0, 50.0);   // 最大加速度 (m/s²)
    std::uniform_real_distribution<double> time_dist(0.0, 86400.0);     // 时间戳 (秒)

    StartYsdd_PresetTrajectoryInput data;

    // 当前状态
    data.current_latitude = lat_dist(rng);
    data.current_longitude = lon_dist(rng);
    data.current_altitude = alt_dist(rng);
    data.current_velocity = vel_dist(rng);
    data.current_heading = heading_dist(rng);

    // 目标状态
    data.target_latitude = lat_dist(rng);
    data.target_longitude = lon_dist(rng);
    data.target_altitude = alt_dist(rng);

    // 性能参数
    data.max_velocity = max_vel_dist(rng);
    data.max_acceleration = max_accel_dist(rng);

    // 时间戳
    data.timestamp = time_dist(rng);

    return data;
}

int main()
{
    // 初始化日志
    LogClient::getInstance().init("mock_0701_input_ysdd", "mock_0701_input_ysdd");

    dds_return_t rc;

    // 1. 初始化DDS参与者（DomainParticipant）
    participant = dds_create_participant(DDS_DOMAIN_DEFAULT, NULL, NULL);
    if (participant < 0)
    {
        std::cerr << "错误：创建参与者失败，原因：" << dds_strretcode(-participant) << std::endl;
        LogClient::getInstance().logError("创建参与者失败，原因：" + std::string(dds_strretcode(-participant)));
        cleanup();
        return EXIT_FAILURE;
    }

    // 2. 创建主题（Topic），名称需与订阅者一致
    topic = dds_create_topic(
        participant,
        &StartYsdd_PresetTrajectoryInput_desc, // IDL生成的类型描述
        "start_ysdd",                          // 主题名称（必须与订阅者一致）
        NULL, NULL);
    if (topic < 0)
    {
        std::cerr << "错误：创建主题失败，原因：" << dds_strretcode(-topic) << std::endl;
        LogClient::getInstance().logError("创建主题失败，原因：" + std::string(dds_strretcode(-topic)));
        cleanup();
        return EXIT_FAILURE;
    }

    // 3. 创建发布者（Publisher）
    publisher = dds_create_publisher(participant, NULL, NULL);
    if (publisher < 0)
    {
        std::cerr << "错误：创建发布者失败，原因：" << dds_strretcode(-publisher) << std::endl;
        LogClient::getInstance().logError("创建发布者失败，原因：" + std::string(dds_strretcode(-publisher)));
        cleanup();
        return EXIT_FAILURE;
    }

    dds_qos_t *qos = dds_create_qos();
    dds_qset_reliability(qos, DDS_RELIABILITY_RELIABLE, DDS_SECS(10));
    dds_qset_history(qos, DDS_HISTORY_KEEP_LAST, 10);

    // 4. 创建数据写入器（DataWriter）
    writer = dds_create_writer(publisher, topic, qos, NULL);
    if (writer < 0)
    {
        std::cerr << "错误：创建写入器失败，原因：" << dds_strretcode(-writer) << std::endl;
        LogClient::getInstance().logError("创建写入器失败，原因：" + std::string(dds_strretcode(-writer)));
        cleanup();
        return EXIT_FAILURE;
    }
    dds_delete_qos(qos);

    std::cout << "DDS预设轨迹发布者已启动，开始发送数据..." << std::endl;
    LogClient::getInstance().logInfo("DDS预设轨迹发布者已启动，开始发送数据...");

    // 5. 循环发送数据（示例：发送10000次，每次间隔1秒）
    // for (int i = 0; i < 10000; i++)
    int i = 0;
    while (1)
    {
        // 生成随机预设轨迹数据
        StartYsdd_PresetTrajectoryInput data = generate_random_trajectory();

        // 发送数据
        rc = dds_write(writer, &data);
        if (rc != DDS_RETCODE_OK)
        {
            std::cerr << "错误：dds_write失败，原因：" << dds_strretcode(-rc) << std::endl;
            cleanup();
            return EXIT_FAILURE;
        }

        // 使用cout打印发送的信息
        std::string info = "已发送预设轨迹数据（第" + std::to_string(i + 1) + "次）:";
        info += "  纬度: " + std::to_string(data.current_latitude) + "°";
        info += "  经度: " + std::to_string(data.current_longitude) + "°";
        info += "  高度: " + std::to_string(data.current_altitude) + " m";
        info += "  速度: " + std::to_string(data.current_velocity) + " m/s";
        info += "  航向: " + std::to_string(data.current_heading) + "°";
        info += "  目标纬度: " + std::to_string(data.target_latitude) + "°";
        info += "  目标经度: " + std::to_string(data.target_longitude) + "°";
        info += "  目标高度: " + std::to_string(data.target_altitude) + " m";
        info += "  最大速度: " + std::to_string(data.max_velocity) + " m/s";
        info += "  最大加速度: " + std::to_string(data.max_acceleration) + " m/s²";
        info += "  时间戳: " + std::to_string(data.timestamp) + " s";

        std::cout << "==============================" << std::endl;
        std::cout << info << std::endl;
        std::cout << "==============================" << std::endl;
        LogClient::getInstance().logInfo(info);

        // 间隔1秒发送
        i++;
        dds_sleepfor(DDS_SECS(1));
    }

    // 清理资源
    cleanup();
    return EXIT_SUCCESS;
}